// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URInverseKinematicsTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.UniversalRobots
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Hardware.UniversalRobots;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test inverse kinematics
    /// </summary>
    [TestClass]
    [DeploymentItem(@"ShoArray.dll")]
    [DeploymentItem(@"MatrixInterf.dll")]
    [DeploymentItem(@"ArrayT.dll")]
    [DeploymentItem(@"libiomp5md.dll")]
    [DeploymentItem(@"mkl.dll")]
    [DeploymentItem(@"msvcp110.dll")]
    [DeploymentItem(@"msvcr110.dll")]
    public class URInverseKinematicsTests
    {
        /// <summary>
        /// Tests the inverse kinematics on a UR5 arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsTestDummyUR5()
        {
            KinematicChain kc = URKinematicConfigurations.CreateUR5KinematicChain();

            // test 1
            DoubleArray joint = DoubleArray.From(new double[] { 21.4, -91.84, 30.93, -38.35, 25.54, 32.83 }).Transpose();
            joint = joint * MathConstants.Degrees2Radians;
            Vector3 rotation = new Vector3(0.7931, 0.8387, -0.6459);
            Vector3 position = new Vector3(-0.17959, -0.26737, 0.90696);
            this.TestUR5SpecificIK(kc, joint, position, rotation);

            // test 2
            joint = DoubleArray.From(new double[] { 48.37, -115.72, -61.64, -55.71, -29.70, 34.29 }).Transpose();
            joint = joint * MathConstants.Degrees2Radians;
            rotation = new Vector3(-0.6101, 1.4992, -2.4409);
            position = new Vector3(0.55183, 0.34894, 0.57965);
            this.TestUR5SpecificIK(kc, joint, position, rotation);

            // test 3
            joint = DoubleArray.From(new double[] { 84.54, -100.53, -110.32, -150.98, -86.30, -3.61 }).Transpose();
            joint = joint * MathConstants.Degrees2Radians;
            rotation = new Vector3(0.1020, 2.1483, 2.0702);
            position = new Vector3(0.16092, 0.48032, 0.20863);
            this.TestUR5SpecificIK(kc, joint, position, rotation);
        }

        /// <summary>
        /// Test inverse kinematics for the UR5 using the joint equivalent generator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsEquivalentGeneratorTestUR5()
        {
            KinematicChain kc = URKinematicConfigurations.CreateUR5KinematicChain();

            for (int i = 0; i < 10; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                // generate equivalent solutions
                List<DoubleArray> solutions = URKinematicConfigurations.UR5EquivalentJointConfigurationGenerator(jointSample);

                // validate solutions
                for (int j = 0; j < solutions.Count; ++j)
                {
                    DoubleArray solution = solutions[j];

                    // compute the end effector pose using forward kinematics
                    Matrix4[] kinematicsValidate = kc.GetJointTransforms(solution);

                    // record test related information
                    string msg_joint = string.Format("failed solution for original joint sample [{0}, {1}, {2}, {3}, {4}, {5}]\n", jointSample[0], jointSample[1], jointSample[2], jointSample[3], jointSample[4], jointSample[5]);
                    string msg_solution_index = string.Format("failed solution index: {0}\n", j);
                    string msg = msg_joint + msg_solution_index;

                    PoseDiff diff = new PoseDiff(kinematicsOriginal[5], kinematicsValidate[5]);
                    Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3, msg);
                }
            }
        }

        /// <summary>
        /// A routine that computes UR5 IK and validates it with data recorded from the teach panel
        /// </summary>
        /// <param name="kc">Kinematic chain</param>
        /// <param name="jointAngles">Joint angles</param>
        /// <param name="position">Recorded position</param>
        /// <param name="scaledRotationVector">Recorded scaled rotation vector representing the orientation</param>
        private void TestUR5SpecificIK(KinematicChain kc, DoubleArray jointAngles, Vector3 position, Vector3 scaledRotationVector)
        {
            Matrix4[] kinematics = kc.GetJointTransforms(jointAngles);

            double angle = scaledRotationVector.Length();
            scaledRotationVector = Vector3.Normalize(scaledRotationVector);
            Matrix4 kinematicsExpected = HomogenousTransform.CreateFromAxisAngle(new AxisAngle(scaledRotationVector, angle));
            kinematicsExpected[0, 3] = position.X;
            kinematicsExpected[1, 3] = position.Y;
            kinematicsExpected[2, 3] = position.Z;

            PoseDiff diff = new PoseDiff(kinematics[6], kinematicsExpected);
            Assert.AreEqual(0, MatrixExtensions.MaxAbsoluteValue(diff), 1e-3);
        }
    }
}
